#include <iostream>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <Utils.h>
#include <vector>
using namespace std;
using namespace lyxutils::abbreviations;

void printUsage(){
    cout<<"usage:pcd2ascii pcd_file|pcd_folder ascii_file|ascii_folder [-i]"<<endl;
    cout<<"      convert pcd file(*.pcd) to ascii file(*.txt) or pcd files in"<<endl;
    cout<<"      a folder to ascii files;"<<endl;
    cout<<"arguments:"<<endl;
    cout<<"  -i        inverse the process to convert ascii file(s) to pcd file(s)"<<endl;
    cout<<"  --rgb     pcd file contains x,y,z,r,g,b"<<endl;
    cout<<"  --normal  pcd file contains normal(treat last three columns as normal vector)"<<endl;
}

typedef pcl::PointXYZ PT;
typedef pcl::PointXYZRGB PTRGB;
typedef pcl::PointNormal PTN;
typedef pcl::PointXYZRGBNormal PTRGBN;
template<typename T>
using Cloud=pcl::PointCloud<T>;

//pcd_type:1-xyz,2-xyzrgb,3-xyznormal
//outFile doen not contain suffix,depends on inverse is true/false
void convert(string inFile,string outFile,int pcd_type,bool inverse=false){
    if(inverse){//ascii to pcd
        if(xos::suffix(inFile)!="txt")return;
        if(xos::suffix(outFile)!="pcd")outFile+=".pcd";
        cout<<"convert "<<xos::abspath(inFile)<<" to "<<xos::abspath(outFile)<<endl;
        if(pcd_type==1){//xyz
            typedef PT Point;
            typedef Cloud<PT> CloudT;
            CloudT::Ptr cloud(new CloudT);
            cout<<"loading..."<<endl;
            xio::read_pts(inFile,cloud->points,",");
            cout<<"saving..."<<endl;
            cloud->width=cloud->points.size();
            cloud->height=1;
            cloud->is_dense=true;
            if(cloud->width>0)pcl::io::savePCDFile(outFile,*cloud);
        }
        else if(pcd_type==2){//xyzrgb
            typedef PTRGB Point;
            typedef Cloud<PTRGB> CloudT;
            CloudT::Ptr cloud(new CloudT);
            cout<<"loading..."<<endl;
            xio::read_ptsrgb(inFile,cloud->points,",");
            cout<<"saving..."<<endl;
            cloud->width=cloud->points.size();
            cloud->height=1;
            cloud->is_dense=true;
            if(cloud->width>0)pcl::io::savePCDFile(outFile,*cloud);
        }
        else if(pcd_type==3){//xyz normal
            typedef PTN Point;
            typedef Cloud<PTN> CloudT;
            CloudT::Ptr cloud(new CloudT);
            cout<<"loading..."<<endl;
            xio::read_csv_customize(inFile,[cloud](const vector<float> &row){
                Point p;
                int sz=row.size();
                p.x=row[0];
                p.y=row[1];
                p.z=row[2];
                p.normal_x=row[sz-3];
                p.normal_y=row[sz-2];
                p.normal_z=row[sz-1];
            },",");
            cout<<"saving..."<<endl;
            cloud->width=cloud->points.size();
            cloud->height=1;
            cloud->is_dense=true;
            if(cloud->width>0)pcl::io::savePCDFile(outFile,*cloud);
        }
        else if(pcd_type==4){//xyz+rgb+normal
            typedef PTRGBN Point;
            typedef Cloud<PTRGBN> CloudT;
            CloudT::Ptr cloud(new CloudT);
            cout<<"loading..."<<endl;
            xio::read_csv_customize(inFile,[cloud](const vector<float> &row){
                Point p;
                int sz=row.size();
                p.x=row[0];
                p.y=row[1];
                p.z=row[2];
                p.r=row[3];
                p.g=row[4];
                p.b=row[5];
                p.normal_x=row[sz-3];
                p.normal_y=row[sz-2];
                p.normal_z=row[sz-1];
            },",");
            cout<<"saving..."<<endl;
            cloud->width=cloud->points.size();
            cloud->height=1;
            cloud->is_dense=true;
            if(cloud->width>0)pcl::io::savePCDFile(outFile,*cloud);
        }
    }
    else{//pcd to ascii
        if(xos::suffix(inFile)!="pcd")return;
        if(xos::suffix(outFile)!="txt")outFile+=".txt";
        cout<<"convert "<<xos::abspath(inFile)<<" to "<<xos::abspath(outFile)<<endl;
        if(pcd_type==1){//xyz
            typedef PT Point;
            typedef Cloud<PT> CloudT;
            CloudT::Ptr cloud(new CloudT);
            cout<<"loading..."<<endl;
            pcl::io::loadPCDFile(inFile,*cloud);
            cout<<"saving..."<<endl;
            ofstream ofs(outFile);
            for(int i=0;i<cloud->points.size();++i){
                Point p=cloud->points[i];
                ofs<<p.x<<","<<p.y<<","<<p.z<<endl;
                xio::consoleProgressBar(i+1,cloud->points.size());
            }
            ofs.flush();
            ofs.close();
        }
        else if(pcd_type==2){//xyzrgb
            typedef PTRGB Point;
            typedef Cloud<PTRGB> CloudT;
            CloudT::Ptr cloud(new CloudT);
            cout<<"loading..."<<endl;
            pcl::io::loadPCDFile(inFile,*cloud);
            cout<<"saving..."<<endl;
            ofstream ofs(outFile);
            for(int i=0;i<cloud->points.size();++i){
                Point p=cloud->points[i];
                ofs<<p.x<<","<<p.y<<","<<p.z<<","<<(int)p.r<<","<<(int)p.g<<","<<(int)p.b<<endl;
                xio::consoleProgressBar(i+1,cloud->points.size());
            }
            ofs.flush();
            ofs.close();
        }
        else if(pcd_type==3){//xyznormal
            typedef PTN Point;
            typedef Cloud<PTN> CloudT;
            CloudT::Ptr cloud(new CloudT);
            cout<<"loading..."<<endl;
            pcl::io::loadPCDFile(inFile,*cloud);
            cout<<"saving..."<<endl;
            ofstream ofs(outFile);
            for(int i=0;i<cloud->points.size();++i){
                Point p=cloud->points[i];
                ofs<<p.x<<","<<p.y<<","<<p.z<<","<<p.normal_x<<","<<p.normal_y<<","<<p.normal_z<<endl;
                xio::consoleProgressBar(i+1,cloud->points.size());
            }
            ofs.flush();
            ofs.close();
        }
        else if(pcd_type==4){//xyz+rgb+normal
            typedef PTRGBN Point;
            typedef Cloud<PTRGBN> CloudT;
            CloudT::Ptr cloud(new CloudT);
            cout<<"loading..."<<endl;
            pcl::io::loadPCDFile(inFile,*cloud);
            cout<<"saving..."<<endl;
            ofstream ofs(outFile);
            for(int i=0;i<cloud->points.size();++i){
                Point p=cloud->points[i];
                ofs<<p.x<<","<<p.y<<","<<p.z<<","<<(int)p.r<<","<<(int)p.g<<","<<(int)p.b
                    <<","<<p.normal_x<<","<<p.normal_y<<","<<p.normal_z<<endl;
                xio::consoleProgressBar(i+1,cloud->points.size());
            }
            ofs.flush();
            ofs.close();
        }
    }
}

int main(int argc, char **argv) {
    xio::CLParser parser;
    parser.parse(argc,argv);
    vector<string> paras=parser.getParameters();
    if(paras.size()<2 || parser.hasOption("h")){
        printUsage();
        return 1;
    }
    int pcd_type=1;
    if(parser.hasOption("rgb")&&parser.hasOption("normal"))pcd_type=4;
    else if(parser.hasOption("rgb"))pcd_type=2;
    else if(parser.hasOption("normal"))pcd_type=3;
    if(xos::directory(xos::abspath(paras[0]))){
        vector<string> files=xos::lsfile(xos::abspath(paras[0]));
        if(!xos::directory(xos::abspath(paras[1]))){
            xos::mkdir(xos::abspath(paras[1]),true);
        }
        for(string f:files){
            convert(f,xos::abspath(paras[1])+"/"+xos::basename(f),pcd_type,parser.hasOption("i"));
        }
    }
    else if(xos::file(xos::abspath(paras[0]))){
        convert(paras[0],xos::abspath(paras[1]),pcd_type,parser.hasOption("i"));
    }
    else{
        cout<<"file:"<<paras[0]<<" not exist!"<<endl;
    }
}
